%data = load('test1.txt');
%data = load('ranging_data.txt');
data = load('result.txt');
%data = data';
pos = data(:,1:3);
%pos = data(2:4,:);
%Angle data
%a = 0:360;
%a = (a/2)*pi/180;

figure(1);
hold off;
plot(0,0,'rx');

hold on;
axis "equal";
%for i=1:5:size(data,1);
for i=270:1:280 %size(data,1);
% Range measurement
r = data(i,:);
r = r(4:183);
a = data(i,:);
a = a(184:end);
%r = r(5:end);
p = pos(i,:);

[v,ind]=find(r>80);
r(ind)=0;

x = r.*cos(a + p(3)) + p(1);
y = r.*sin(a + p(3)) + p(2);

plot(x,y,'.b');
plot(pos(1),pos(2),'.r');
sleep(0.15)
end;
